clc; close all; clear all;

dt = 0.2;

%% Unicycle Model
sys = ICtSystem(...
    'StateEquation', @(t,x,u,varargin) [
    u(1)*cos(x(3));
    u(1)*sin(x(3));
    u(2)],...
    'nx',3,'nu',2 ...
);

sys.initialCondition = {[15;15;-pi/2],-[15;15;-pi/2],[15;-15;pi],[-15;15;-pi/2]};

auxiliaryControlLaw =  TrackingController_ECC13(... % <<< attached to the realSystem
    @(t) 10*[sin(0.1*t); cos(0.1*t)] , ... % c
    @(t)    [cos(0.1*t);-sin(0.1*t)] , ... % cDot
    eye(2)                           , ... % K
    [1;0] );

e = @(t,x)auxiliaryControlLaw.computeError(t,x);

mpcOp = ICtMpcOp( ...
    'System'               , sys,...
    'HorizonLength'        , 3*dt,...
    'StageConstraints'     , BoxSet( -[1;1],4:5,[1;1],4:5,5),... % on the variable z=[x;u];
    'StageCost'            , @(t,x,u,varargin) e(t,x)'* e(t,x),...
    'TerminalCost'         , @(t,x,varargin) 0.3333*(e(t,x)'* e(t,x))^(3/2)...
    );

dtMpcOp      = DiscretizedMpcOp(mpcOp,dt);

dtRealSystem = DiscretizedSystem(sys,dt);

dtRealSystem.controller = MpcController(...
    'MpcOp'       , dtMpcOp ,...
    'MpcOpSolver' , FminconMpcOpSolver('MpcOp', dtMpcOp,'UseSymbolicEvaluation',1) ...
    );

va = VirtualArena(dtRealSystem,...
    'StoppingCriteria'  , @(t,sysList)t>70/dt,...
    'PlottingStep'      , 1/dt, ...
    'StepPlotFunction'  , @ex01StepPlotFunction ...
    );

log = va.run();
